from geometry_msgs.msg import PoseStamped,Pose
#!/usr/bin/env python
import rospy
import string
import math
import time
import sys
from nav_msgs.msg import Odometry
from std_msgs.msg import String
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped
from gazebo_msgs.msg import ModelStates

class init_time:
    def __init__(self):
        self.time_flag = 0
        self.get_flag = 0
        self.cur_x = 0
        self.cur_y = 0
        self.init_point_x = 0.01
        self.start_time = 0
        self.end_time = 0
        self.index_racecar = 0
        self.pose_gazebo = rospy.Subscriber('/gazebo/model_states',ModelStates,self.getPose_gazebo,queue_size=10)


    def getPose_gazebo(self,data):
        if self.get_flag == 0:
            for i in range(len(data.name)):
                if data.name[i] == 'racecar':
                    self.get_flag=1
                    self.index_racecar = i
        self.cur_x = data.pose[self.index_racecar].position.x
        self.cur_y = data.pose[self.index_racecar].position.y
        if self.cur_x > self.init_point_x and self.time_flag == 0:
            self.time_flag +=1
            self.start_time=rospy.get_time()
            rospy.loginfo("Initial start couting time")
        if self.cur_x < self.init_point_x and self.time_flag ==1:
            self.time_flag +=1
            self.end_time = rospy.get_time()
            rospy.loginfo("finished the game!! ending couting time")
            interval = self.end_time-self.start_time
            print(interval)


if __name__ == '__main__':
    rospy.init_node("init_time")
    init_time()
    r = rospy.Rate(100)
    r.sleep()
    rospy.spin()